#include <std_msgs/String.h>

// 要点：
// ①时刻保证身体部分竖直
// ②时刻保证脚掌平行地面
// ③行走是身体高度不变
// ④速度为机器人质心速度
class Walk
{
    public:
    Walk();
    ~Walk();
    void Init(float *joints, int height, bool back=false);
    bool Inverse(float *left_position, float *right_position, float *joints, bool back=false);
    bool walk(float *joints);
    void back(float step_len, float *joints);
    bool Back(float *joints);
    std::string get_status();
    private:
    float hip_pitch_bias;
    float big_len;    // 大腿长度
    float small_len;  // 小腿长度
    float foot_height;  // 脚高度
    float delta_z;      // 当前状态与站直时的身体高度差
    float delta_x;
    float left_leg_position[3];
    float right_leg_position[3];
    float delta_x_count;
    std::string status;
};